// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SLAM2DParticleFilter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using MicrosoftResearch.Infer.Distributions;

    /// <summary>
    /// SLAM particle filter. 
    /// </summary>
    /// <typeparam name="TObservation">The type of horizontal depth profile (kinect or laser) to consume</typeparam>
    /// <typeparam name="TDriveFeedbackState">Drive feedback state which provides the motion model</typeparam>
    public class SLAM2DParticleFilter<TObservation, TDriveFeedbackState> : ParticleFilter<SLAM2DParticle, TDriveFeedbackState, TObservation>
        where TObservation : IHorizontalDepthProfile
        where TDriveFeedbackState : IDriveState
    {
        /// <summary>
        /// The random generator used to decide update frequency
        /// </summary>
        private Random rand;

        /// <summary>
        /// Initializes a new instance of the SLAM2DParticleFilter class.
        /// </summary>
        /// <param name="map">The map to start with</param>
        /// <param name="particleCount">The count of particles to maintain.</param>
        /// <param name="startPose">The desired start position of the robot on the map, in map coordinates. This is useful in properly centering the acquired map.</param>
        public SLAM2DParticleFilter(GlobalMap map, int particleCount, Pose2D startPose)
            : base(particleCount, "SLAM2DParticleFilter")
        {
            this.EnableRandomResampling = false;
            this.Particles[0] = new SLAM2DParticle(map, startPose); 
            this.rand = new Random();
            this.ResampleAll();
        }

        /// <summary>
        /// Updates all particles based on the specified motion update and observation. 
        /// </summary>
        /// <param name="update">The motion update to apply to all particles.</param>
        /// <param name="observation">The observation made after the motion completed. This observation is used to determine the fitness of each particle.</param>
        public override void Update(TDriveFeedbackState update, TObservation observation)
        {
            base.Update(update, observation);

            // update the map once for all particles (this is a simple PF, not an RBPF)
            // to turn this into an RBPF, simply move hte map update into UpdateSample and change CloneSample to tell each particle to also clone the map 
            SLAM2DParticle priorState = this.GetHighestLikelihoodParticle();
            priorState.Map.Update(observation, priorState.Pose);
        }

        /// <summary>
        /// Clones an existing sample.
        /// </summary>
        /// <param name="priorState">The existing particle.</param>
        /// <returns>A clone of the existing particle.</returns>
        protected override SLAM2DParticle CloneSample(SLAM2DParticle priorState)
        {
            // by not cloning the map (param = false) we maintain a simple PF instead of an RBPF
            SLAM2DParticle newParticle = priorState.Clone(false);
            return newParticle;
        }

        /// <summary>
        /// Updates the given particle based on motion and observation and returns the probability that the state specified by the particle would result in the given observation.
        /// </summary>
        /// <param name="priorState">The old particle state.</param>
        /// <param name="update">The update to apply.</param>
        /// <param name="observation">The observation, assumed noisy.</param>
        /// <param name="weight">The current weight of the particle</param>
        /// <param name="fitnessToObservation">The probability of a fit, in log-likelihood form.</param>
        /// <returns>The updated particle.</returns>
        protected override SLAM2DParticle UpdateSample(
            SLAM2DParticle priorState,
            TDriveFeedbackState update,
            TObservation observation,
            double weight,
            out double fitnessToObservation)
        {
            TDriveFeedbackState previousUpdate = (TDriveFeedbackState)priorState.DriveFeedbackState;
            TDriveFeedbackState currentState = update;            
            if (previousUpdate != null)
            {                
                Pose2D previousPoseInMeters = priorState.Map.MapCellsToMeters(priorState.Pose);                
                Pose2D updatedPoseInMeters = currentState.SamplePose(previousPoseInMeters, previousUpdate);

                priorState.Pose = priorState.Map.MetersToMapCells(updatedPoseInMeters);                                
            }

            // compute the fitness 
            IHorizontalDepthProfile surface;
            Pose2D bestPose = priorState.Pose;
            fitnessToObservation = priorState.Map.Match(observation, ref bestPose, true /*allowExploration*/, out surface);
            priorState.SurfaceFromMap = surface;
            priorState.Pose = bestPose;
            
            // record the properties used in visualization 
            priorState.SingleIterationWeight = fitnessToObservation;
            priorState.Weight = weight;
            priorState.UpdateCount++;

            priorState.DriveFeedbackState = currentState;
            return priorState;
        }
    }
}
